// #include "aim_energy/WindMill.hpp"

// namespace WINDMILL
// {
//     void windMill::SmallPredict(float v, float pitch_deg)
//     {

//         double dist = HitDist(now_angle);
//         double t_arrive1 = CalTime(dist, v, pitch_deg);
//         double t_sum = t_arrive1;
//         double t_arrive2 = 0.0;
//         double next_angle = now_angle;
//         double dt;
//         for (int i = 0; i < 20; i++)
//         {
//             dt = t_arrive1 - t_arrive2;
//             t_sum += dt;
//             if (dt < 0.0005)
//                 break;
//             next_angle = SPreNextAngle(next_angle, dt);
//             dist = HitDist(next_angle);
//             t_arrive2 = t_arrive1;
//             t_arrive1 = CalTime(dist, v, pitch_deg);
//         }
//         pre_angle = SPreNextAngle(now_angle, t_sum);
//         pre_point = CalPoint(center, params.R, pre_angle);
//         pre_point = hit_point;
//         cv::circle(src, pre_point, 3, cv::Scalar(0, 0, 255), -1);
//     }

//     void windMill::BigPredict(float v, float pitch_deg)
//     {

//         if (anglevelocity_rad.size() < 50)
//         {
//             return;
//         }
//         else
//         {
//             FaiFit();
//             double dist = HitDist(now_angle);
//             double t_arrive1 = CalTime(dist, v, pitch_deg);
//             double t_sum = t_arrive1;
//             double t_arrive2 = 0.0;
//             double next_angle = now_angle;
//             double dt;
//             double t0 = now_t;
//             for (int i = 0; i < 20; i++)
//             {
//                 dt = t_arrive1 - t_arrive2;
//                 if (dt < 0.0005)
//                     break;
//                 t_sum += dt;
//                 next_angle = BPreNextAngle(next_angle, t0, dt);
//                 t0 += dt;
//                 dist = HitDist(next_angle);
//                 t_arrive2 = t_arrive1;
//                 t_arrive1 = CalTime(dist, v, pitch_deg);
//             }
//             pre_angle = BPreNextAngle(now_angle, now_t, t_sum);
//             pre_point = CalPoint(center, params.R, pre_angle);
//             // pre_point = hit_point;
//             cv::circle(src, pre_point, 5, cv::Scalar(0, 0, 255), -1);
//         }
//     };

//     void windMill::FaiFit()
//     {
//         // std::cout << "=========================" << std::endl;
//         for (int i = 0; i < 150; i++)
//         {
//             double fai_iterator = spd.fai;
//             iterator++;
//             double diff1_sum = FaiDiff(fai_iterator, time, anglevelocity_rad);
//             double loss_sum = LossFun(fai_iterator, time, anglevelocity_rad);
//             fai_iterator -= 3 * (exp(-0.001 * iterator)) * abs(loss_sum - last_loss) * (diff1_sum);
//             if (abs(loss_sum - last_loss) < 1e-10)
//             {
//                 break;
//             }
//             // std::cout << spd.fai << "," << fai_iterator << std::endl;
//             if (abs(spd.fai - fai_iterator) > 0.8)
//             {
//                 for (int j = 0; j < time.size(); j++)
//                 {
//                     // std::cout << anglevelocity_rad[j] << "," << time[j] << std::endl;
//                 }
//             }
//             double weight = last_loss / (loss_sum + last_loss);
//             spd.fai = (1 - weight) * spd.fai + weight * fai_iterator;
//             last_loss = loss_sum;
//         }
//         while (spd.fai < 0)
//         {
//             spd.fai += 2 * Pi;
//         }
//         while (spd.fai > 2 * Pi)
//         {
//             spd.fai -= 2 * Pi;
//         }
//     }
// }
